%% setup.m
% UAV Nonlinear Simulation setup
% This script will setup the nonlinear simulation (UAV_NL.mdl) and call
% trim and linearization routines. Select the desired aircraft here in this
% script, via the "UAV_config()" function call.
% Calls: 
%        trim_uav.m
%        linearize_UAV.m
clear all
clc
data_aero_vtp;
%% Set aircraft initial conditions 
% Note: these are NOT the trim condition targets. If the trim fails to
% coverge, try using different initial conditions. Also note that the trim
% function will overwrite these initial conditions with the trimmed
% conditions.
% Set the initial model inputs
TrimCondition.Inputs.da   = 0; % rad
TrimCondition.Inputs.de   = -1.1;     % rad
TrimCondition.Inputs.dr   = 0;     % rad
TrimCondition.Inputs.df   = 0;    % rad
TrimCondition.Inputs.dT   = 0.4;     % nd, 0 to 1
% Set initial state values
TrimCondition.XIni      = Xme_0;   % Initial Position in Inertial Frame [Xe Ye Ze], [m]
TrimCondition.VbIni     = Uvw0; % Initial Body Frame velocities [u v w], [m/s]
TrimCondition.EulerIni  = euler_0; % Initial Euler orientation [roll,pitch,yaw] [rad]
TrimCondition.pqrIni    = pqr0;      % Initial Body Frame rotation rates [p q r], [rad/s]
%% Trim aircraft to a specific flight condition
% Set the trim targets here. See trim_UAV for complete list of target
% variables. Sideslip angle (beta), flight path angle (gamma) and flap
% setting default to zero. If a fixed control input setting is desired,
% specify as a target.
TrimCondition.target = struct('V',37,'gamma',0); % straight and level, (m/s, rad)
%TrimCondition.target = struct('V',37,'gamma',0); 
%TrimCondition.target.da = 0;
%TrimCondition.target.dr = 0;
%TrimCondition.target.df = 0;
%TrimCondition.target.dT = 0.5;
% TrimCondition.target = struct('V',37.1,'gamma',7.5/180*pi); % level climb, (m/s, rad)
% TrimCondition.target = struct('V',17,'gamma',0,'psidot',20/180*pi); % level turn, (m/s, rad, rad/sec)
% TrimCondition.target = struct('V',17,'gamma',5/180*pi,'psidot',20/180*pi); % climbing turn, (m/s, rad, rad/sec)
% TrimCondition.target = struct('V',17,'gamma',0,'beta',5/180*pi); % level steady heading sideslip, (m/s, rad, rad)
% Find the trim solution
[TrimCondition,OperatingPoint] = trim_uav(TrimCondition,'VTP_airframe');
%% Linearize about the operating point
[longmod,spmod,latmod,linmodel]=linearize_vtp(OperatingPoint);
save('VTP_ssmodes.mat','longmod','spmod','latmod','linmodel');



